"""
步态控制器
用于管理和执行六足机器人的步态模式
"""

import time
from typing import Dict, Tuple, Optional
from .gait_generator import GaitGenerator, GaitParams
from ..control import MotionController


class GaitController:
    """步态控制器类"""

    def __init__(
        self,
        motion_controller: MotionController,
        leg_config: Dict[int, Tuple[float, float, float]],
    ):
        """
        初始化步态控制器

        Args:
            motion_controller: 运动控制器实例
            leg_config: 腿部配置，格式为 {leg_id: (x, y, z)}
        """
        self.motion_controller = motion_controller
        self.gait_generator = GaitGenerator(leg_config)
        self.current_gait: Optional[str] = None
        self.is_running = False
        self.last_update_time = 0.0

    def start_gait(
        self, gait_name: str, direction: Tuple[float, float] = (1.0, 0.0)
    ) -> None:
        """
        开始执行指定步态

        Args:
            gait_name: 步态名称（'tripod', 'wave', 'ripple'）
            direction: 运动方向 (x, y)
        """
        if gait_name not in ["tripod", "wave", "ripple"]:
            raise ValueError(f"Unknown gait: {gait_name}")

        self.current_gait = gait_name
        self.is_running = True
        self.last_update_time = time.time()

        # 设置默认步态参数
        params = GaitParams()
        self.gait_generator.set_params(params)

        # 开始步态循环
        while self.is_running:
            self._update_gait(direction)
            time.sleep(0.01)  # 控制更新频率

    def stop_gait(self) -> None:
        """停止当前步态"""
        self.is_running = False
        self.current_gait = None

    def set_gait_params(self, params: GaitParams) -> None:
        """设置步态参数"""
        self.gait_generator.set_params(params)

    def _update_gait(self, direction: Tuple[float, float]) -> None:
        """更新步态"""
        current_time = time.time()
        dt = current_time - self.last_update_time
        self.last_update_time = current_time

        # 计算时间参数
        t = (
            current_time % self.gait_generator.params.cycle_time
        ) / self.gait_generator.params.cycle_time

        # 根据当前步态生成目标位置
        if self.current_gait == "tripod":
            positions = self.gait_generator.tripod_gait(t, direction)
        elif self.current_gait == "wave":
            positions = self.gait_generator.wave_gait(t, direction)
        elif self.current_gait == "ripple":
            positions = self.gait_generator.ripple_gait(t, direction)
        else:
            return

        # 更新舵机位置
        for leg_id, pos in positions.items():
            self.motion_controller.move_servo(leg_id, pos)

    def move_forward(self, speed: float = 1.0) -> None:
        """向前移动"""
        self.start_gait("tripod", (speed, 0.0))

    def move_backward(self, speed: float = 1.0) -> None:
        """向后移动"""
        self.start_gait("tripod", (-speed, 0.0))

    def turn_left(self, speed: float = 1.0) -> None:
        """向左转向"""
        self.start_gait("tripod", (0.0, speed))

    def turn_right(self, speed: float = 1.0) -> None:
        """向右转向"""
        self.start_gait("tripod", (0.0, -speed))

    def set_body_pose(
        self, height: float, roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0
    ) -> None:
        """设置身体姿态"""
        params = self.gait_generator.params
        params.body_height = height
        params.body_roll = roll
        params.body_pitch = pitch
        params.body_yaw = yaw
        self.gait_generator.set_params(params)
